#include "BSP.h"
//50Hz 1000~2000us=3000~6000
// 周期49999，分频后时钟

#define POS_CENT    4500
#define POS_RANG    1500

/** @brief  舵机初始化
 *  @note   
 *  @author 江榕煜（2021.6.8）
 *  @param  None
 *  @retval None
**/
void steerEngine_Init(void)
{
    HAL_TIM_PWM_Start(&htim16,   TIM_CHANNEL_1); // 启动PWM输出
    HAL_TIM_PWM_Start(&htim17,   TIM_CHANNEL_1); // 启动PWM输出
    steerEngine_SetPos(0,50);    // Pitch   
    steerEngine_SetPos(1,-100);
}

/** @brief  舵机位置设置
 *  @note   使用前必使用 steerEngine_Init() 方法
 *          位置百分比范围：-100%~+100%
 *  @author 江榕煜（2021.4.2）
 *  @param
 *          channel(uint32_t)   电机通道号
 *          duty(float)         占空比
 *  @retval 
 *          None
**/
void steerEngine_SetPos(uint32_t channel,float duty)
{
    duty = duty*POS_RANG/100 + POS_CENT;
    if(duty > POS_CENT+POS_RANG)        duty = POS_CENT+POS_RANG;
    else if(duty < POS_CENT-POS_RANG)   duty = POS_CENT-POS_RANG;
    switch(channel)
    {
        case(0):
            htim16.Instance->CCR1 = duty;   // Yaw轴
            break;
        case(1):
            htim17.Instance->CCR1 = duty;   // Pitch轴
            break;
    }
}
